// (c) Copyright 2013-2014 Stephen Gold <sgold@sonic.net>
// Distributed under the terms of the GNU General Public License

/*
 This file is part of the Bats Game.

 The Bats Game is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by the
 Free Software Foundation, either version 3 of the License, or (at your
 option) any later version.

 The Bats Game is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 for more details.

 You should have received a copy of the GNU General Public License
 along with the Bats Game.  If not, see <http://www.gnu.org/licenses/>.
 */
package bats.unit;

import bats.scene.Physics;
import bats.scene.SceneNode;
import bats.scene.WorldNode;
import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;
import com.jme3.scene.Spatial;
import java.util.logging.Level;
import java.util.logging.Logger;
import jme3utilities.SimpleControl;
import jme3utilities.math.MyMath;
import jme3utilities.math.MyVector3f;
import jme3utilities.math.VectorXZ;

/**
 * A simple control to guide a missile-armed unit in an aggressive fashion using
 * the unit's launch and walk controls.
 * <p>
 * Each instance is disabled at creation.
 *
 * @author Stephen Gold <sgold@sonic.net>
 */
public class AggressiveControl
        extends SimpleControl {
    // *************************************************************************
    // constants

    /**
     * message logger for this class
     */
    final private static Logger logger =
            Logger.getLogger(AggressiveControl.class.getName());
    // *************************************************************************
    // fields
    /**
     * aim tolerance (meters, &gt;0) TODO for each type of target
     */
    private float aimTolerance = 0.1f;
    /**
     * minimum optimal range (in meters, &ge;0) TODO for each type of target
     */
    private float minRange = 3f;
    /**
     * maximum optimal range (in meters, &ge;0) TODO for each type of target
     */
    private float maxRange = 6f;
    /*
     * TODO: priority and maxRange for each type of target
     */
    /**
     * cached launch control for the spatial
     */
    private LaunchControl launchControl = null;
    /**
     * cached translation control for the spatial
     */
    private MoveControl moveControl = null;
    // *************************************************************************
    // constructors

    /**
     * Instantiate a disabled control.
     */
    public AggressiveControl() {
        super.setEnabled(false);
    }
    // *************************************************************************
    // SimpleControl methods

    /**
     * Callback invoked when the spatial's geometric state is about to be
     * updated, once per frame while attached and enabled.
     *
     * @param updateInterval time interval between updates (in seconds, &ge;0)
     */
    @Override
    protected void controlUpdate(float updateInterval) {
        super.controlUpdate(updateInterval);
        if (spatial == null || !SceneNode.find(spatial).isEnabled()) {
            return;
        }
        /*
         * Cache references to the slave controls.
         */
        if (launchControl == null) {
            launchControl = spatial.getControl(LaunchControl.class);
        }
        if (moveControl == null) {
            moveControl = spatial.getControl(OtoControl.class);
        }
        /*
         * Tell the move control to forget all prior guidance.
         */
        moveControl.newGuidanceInput();
        float elevationAngle = 0f;
        /*
         * Select a target.
         */
        WorldNode world = WorldNode.find(spatial);
        Targets targets = world.getPickables().getTargets();
        Spatial missile = launchControl.getMissile();
        Vector3f start;
        if (missile == null) {
            start = spatial.getWorldTranslation();
        } else {
            start = missile.getWorldTranslation();
        }
        targets.scoreNearest(start, minRange);
        targets.exclude(spatial);
        // TODO visibility test
        // TODO alignment test
        // TODO weight scores by type of unit
        // TODO hysteresis

        Target target = targets.select();
        if (target != null) {
            /*
             * Turn to face the target.
             */
            Vector3f end = target.getWorldLocation();
            Vector3f offset = end.subtract(start);
            VectorXZ targetDirection = MyVector3f.horizontalDirection(offset);
            // TODO calculate flight time in order to lead moving targets
            requestDirection(targetDirection);
            /*
             * Move toward optimum range.
             */
            SceneNode scene = SceneNode.find(spatial);
            float scaleFactor = scene.getScaleFactor();
            float range = offset.length() * scaleFactor; // meters
            float optimumRange = FastMath.clamp(range, minRange, maxRange);
            float approach = range - optimumRange; // meters
            VectorXZ destinationOffset = targetDirection.mult(approach);
            moveControl.requestOffset(destinationOffset);
            /*
             * Check missile status.
             */
            if (launchControl.isReadyToLaunch()) {
                /*
                 * The unit has both a ready missile and a target.
                 * Consider launching the missile.
                 */
                VectorXZ direction =
                        world.getOtos().getMapPelvicDirection(spatial);
                float sine = direction.directionError(targetDirection);
                float aimError = FastMath.abs(sine * range); // meters
                if (aimError < aimTolerance) {
                    float launchSpeed = launchControl.getMaxLaunchSpeed();
                    Float angle = launchAngleFrom(missile, end, launchSpeed);
                    if (angle == null) {
                        logger.log(Level.WARNING, "no launch angle found");
                    } else {
                        elevationAngle = angle;
                        logger.log(Level.INFO,
                                "{0} requests launch: speed={1}, "
                                + "elevationAngle={2}",
                                new Object[]{
                            spatial.getName(),
                            launchSpeed,
                            elevationAngle
                        });
                        launchControl.requestLaunch(launchSpeed,
                                elevationAngle);
                    }
                }
            }
        }
        if (missile == null) {
            /*
             * Keep a missile loaded (even when there's no target).
             */
            launchControl.requestLoad(elevationAngle);
        }
    }
    // *************************************************************************
    // private methods

    /**
     * Calculate the ballistic launch angle for a missile with a specified
     * launch speed.
     *
     * @param from starting spatial (not null)
     * @param toPos ending point (not null)
     * @param launchSpeed (in meters per second, &ge;0)
     *
     * @return angle above the horizonal (in radians, -PI/2 to PI/2) or null if
     * no solution found
     */
    private Float launchAngleFrom(Spatial from, Vector3f toPos,
            float launchSpeed) {
        Vector3f fromPos = from.getWorldTranslation().clone();
        Vector3f offset = toPos.subtract(fromPos);
        float scaleFactor = SceneNode.find(spatial).getScaleFactor();
        offset.multLocal(scaleFactor); // convert to meters

        double dx2 = MyMath.sumOfSquares(offset.x, offset.z);
        float dx = (float) Math.sqrt(dx2); // horizontal range (in meters)
        float dy = offset.y; // increase in elevation (in meters)
        /*
         * Tweak the acceleration to adjust for observed errors.
         */
        float halfAcceleration = Physics.gravityMagnitude / 2f - 0.04f;
        double v0 = launchSpeed;
        /*
         * Solve a quadratic equation for the tangent of the launch angle.
         */
        float a = halfAcceleration * (float) (dx2 / (v0 * v0));
        assert a > 0f : a;
        float b = -dx;
        float c = a + dy;
        double discriminant = MyMath.discriminant(a, b, c);
        if (discriminant < 0.0) {
            logger.log(Level.INFO, "No solution found.");
            return null;
        }
        /*
         * Use the lesser of the two roots for the tangent of the launch angle.
         */
        float root = (-b - (float) Math.sqrt(discriminant)) / (2f * a);
        float angle = FastMath.atan(root);
        logger.log(Level.INFO, "launch angle solution = {0} degrees",
                angle * FastMath.RAD_TO_DEG);
        return Float.valueOf(angle);
    }

    /**
     * Request that the unit turn to face the specified direction.
     *
     * @param direction unit vector (unaffected)
     */
    private void requestDirection(VectorXZ direction) {
        assert direction.isUnitVector();

        VectorXZ mirroredGoal = moveControl.copyDirectionGoal().mirrorZ();
        VectorXZ turn = direction.rotate(mirroredGoal);
        moveControl.requestTurn(turn);
    }
}